#include "tracking.h"

#define MAX_NUM 9999999999.0
#define MIN_NUM -9999999999.0
#define THR_MS 0.5
#define R_SIZE 100000
#define F_SIZE 100

dres tracking_dp(dres dres_in, double c_en, double c_ex, double c_ij, double betta, double thr_cost, int max_it, int nms_in_loop){
	double* f2_cost = new double[F_SIZE];
	int dnum = dres_in.nBox;
	dres_in.c = new double[dnum];
	for(int i = 0; i < dnum; i++)
		dres_in.c[i] = betta - dres_in.r[i];
	
	double min_c = MIN_NUM;
	int it = 0, k = 0;
	int *inds_all, *id_s, *redo_nodes;
	redo_nodes = new int[dnum];
	inds_all = new int[R_SIZE];
	id_s = new int[R_SIZE];

	for(int i = 0; i < dnum; i++){
		redo_nodes[i] = i;
	}
	
	dres_in.dp_c = new double[dnum];
	dres_in.dp_link = new int[dnum];
	dres_in.orig = new int[dnum];
	for (int i = 0; i < dnum; i++){
		dres_in.dp_c[i] = 0;
		dres_in.dp_link[i] = 0;
		dres_in.orig[i] = 0;
	}

	int size_r = dnum, size_i = 0;
	while((min_c<thr_cost) && (it<max_it)){
		it = it + 1;
		cout << it << "  " << min_c << endl;
		if(it%50==0){
			int stopHere = 1;
		}
		
		for (int i = 0; i < size_r; i++){
			dres_in.dp_c[redo_nodes[i]] = dres_in.c[redo_nodes[i]] + c_en;
			dres_in.dp_link[redo_nodes[i]] = 0;
			dres_in.orig[redo_nodes[i]] = redo_nodes[i];
		}
		
		for (int ii = 0; ii < size_r; ii++){
			int i = redo_nodes[ii];
			vector<int> f2 = dres_in.nei[i].inds;
			if(f2.size() == 0) continue;
			
			for(int j = 0; j < f2.size(); j++){
				f2_cost[j] = c_ij + dres_in.c[i] + dres_in.dp_c[f2[j]];
			}
			
			double min_cost = MAX_NUM;
			int min_idx = 0;
			for(int j = 0; j < f2.size(); j++){
				if(min_cost > f2_cost[j]){
					min_cost = f2_cost[j];
					min_idx = j;
				}
			}
			int min_link = f2[min_idx];
			if(dres_in.dp_c[i] > min_cost){
				dres_in.dp_c[i] = min_cost;
				dres_in.dp_link[i] = min_link;
				dres_in.orig[i] = dres_in.orig[min_link];
			}
		}
		
		double min_c_temp = MAX_NUM;
		int min_idx;
		for(int j = 0; j < dnum; j++){
			if (min_c_temp > dres_in.dp_c[j]){
				min_c_temp = dres_in.dp_c[j];
				min_idx = j;
			}
		}
		min_c = min_c_temp + c_ex;
		
		vector<int> inds;
		int k1 = 0;
		while(min_idx != 0){
			k1 = k1 + 1;
			inds.push_back(min_idx);
			min_idx = dres_in.dp_link[min_idx];
		}
		
		for(int j = 0; j < inds.size(); j++){
			inds_all[size_i] = inds[j];
			id_s[size_i] = it;
			size_i++;
		}
		k = k + inds.size();
		
		vector<int> supp_inds;
		size_r = 0;

		if(nms_in_loop){
			supp_inds = nms_aggressive(dres_in, inds, THR_MS);
			
			vector<int> origs, origs_temp;
			for(int j = 0; j < supp_inds.size(); j++){
				origs_temp.push_back(dres_in.orig[supp_inds[j]]);
			}
			sort(origs_temp.begin(), origs_temp.end());
			int previous = -1;
			for(int j = 0; j < origs_temp.size(); j++){
				if(origs_temp[j] != previous){
					origs.push_back(origs_temp[j]);
					previous = origs_temp[j];
				}
			}
			
			size_r = find_memberidx(dres_in.orig, dnum, origs, redo_nodes);
		}else{
			size_r = 0;
			supp_inds = inds;
			int origs = inds.back();
			for(int j = 0; j < dres_in.nBox; j++){
				if(dres_in.orig[j] == origs){
					redo_nodes[size_r] = j;
					size_r++;
				}
			}
		}
		size_r = setdiff(redo_nodes, size_r, supp_inds);
		for(int j = 0; j < supp_inds.size(); j++){
			dres_in.dp_c[supp_inds[j]] = MAX_NUM;
			dres_in.c[supp_inds[j]] = MAX_NUM;
		}
		
		dres_in.min_cs.push_back(min_c);
	}

	
	dres result;
	result.nBox = k;
	result.nFrame = dres_in.nFrame;
	result.min_cs = dres_in.min_cs;
	result.x = new double[k];
	result.y = new double[k];
	result.w = new double[k];
	result.h = new double[k];
	result.r = new double[k];
	result.fr = new int[k];
	result.nei = new neighbor[k];
	result.c = new double[k];
	result.dp_c = new double[k];
	result.dp_link = new int[k];
	result.orig = new int[k];
	result.id = new int[k];
	for(int j = 0; j < k; j++){
		int idx = inds_all[j];
		result.x[j] = dres_in.x[idx];
		result.y[j] = dres_in.y[idx];
		result.w[j] = dres_in.w[idx];
		result.h[j] = dres_in.h[idx];
		result.r[j] = dres_in.r[idx];
		result.fr[j] = dres_in.fr[idx];
		result.nei[j] = dres_in.nei[idx];

		result.c[j] = dres_in.c[idx];
		result.dp_c[j] = dres_in.dp_c[idx];
		result.dp_link[j] = dres_in.dp_link[idx];
		result.orig[j] = dres_in.orig[idx];
		result.id[j] = id_s[j];
	}

	delete[]inds_all;
	delete[]id_s;
	delete[]redo_nodes;

	return result;
}